from .path_helper import PrismBuilder, PathPrism, PathCalibrator, PathSplit
import numpy as np

from clamp_generation import maze
from clamp_generation.maze import Maze
from clamp_generation.Astar3D import Weighted_A_star
from jxq.coordinate import G2L
import copy
import trimesh


class BlockUtil():
    def __init__(self, points_pair, dx=65, dz=20):
        self.points_pair = points_pair
        self.dx = dx
        self.dz = dz

    def get_entry(self, i):
        pair = self.points_pair[i]
        start = pair['start']
        target = pair['target']
        axis = pair['start_axis']
        normal = pair['start_normal']
        if 'adjust_times' not in pair:
            adjust_times = 1
        else:
            adjust_times = pair['adjust_times']
        if isinstance(target, np.ndarray):
            dire = target - start
        elif isinstance(target, list):
            target = np.mean(target, axis=0)
            dire = target - start
        if np.inner(dire, axis) > 0:
            ax = axis
        else:
            ax = -axis

        entry_dict = {}
        if pair["rps_type"] is None:
            entry = start + ax * self.dx / 2 + normal * self.dz / 2

            entry_dict = {
                "point": entry,
                "axis": ax,
                "normal": normal,
                "adjust_times": adjust_times
            }
        elif pair["rps_type"] == "伸缩销":
            vertical = np.cross(ax, normal)
            vertical = vertical / np.linalg.norm(vertical)
            erntry = start + vertical * 50 / 2 + normal * 25 / 2
            entry_dict = {
                "point": erntry,
                "axis": vertical,
                "vertical": ax,
                "normal": normal,
                "adjust_times": adjust_times,
                "rps_type": "伸缩销"
            }
        return entry_dict

    def get_exit(self, i):
        pair = self.points_pair[i]
        start = pair['start']
        target = pair['target']
        axis = pair['target_axis']
        normal = pair['target_normal']

        if isinstance(target, np.ndarray):
            dire = start - target
        elif isinstance(target, list):
            tar = np.mean(target, axis=0)
            dire = start - tar
        # dire = start - target
        exit_dict = {}
        if axis is None:
            exit_ = target
            exit_dict = {
                "point": exit_,
                "axis": axis,
                "normal": normal
            }
            return exit_dict
        elif isinstance(axis, np.ndarray):
            if np.inner(dire, axis) > 0:
                ax = axis
            else:
                ax = -axis
            if pair["rps_type"] is None:
                exit_ = target + ax * self.dx / 2 + normal * self.dz / 2
                exit_dict = {
                    "point": exit_,
                    "axis": ax,
                    "normal": normal
                }
            elif pair["rps_type"] == "伸缩销":
                vertical = np.cross(ax, normal)
                vertical = vertical / np.linalg.norm(vertical)
                exit_ = target + vertical * 50 / 2 + normal * 25 / 2
                exit_dict = {
                    "point": exit_,
                    "axis": vertical,
                    "vertical": ax,
                    "normal": normal
                }
        elif isinstance(axis, list):
            exit_dict = {
                "point": [],
                "axis": [],
                "normal": []
            }
            dires_list = [(start - target[k]) for k in range(len(target))]
            for i in range(len(axis)):
                if np.inner(dires_list[i], axis[i]) > 0:
                    ax = axis[i]
                else:
                    ax = -axis[i]
                if pair["rps_type"] is None:
                    exit_ = target[i] + ax * self.dx / 2 + np.array(normal[i]) * self.dz / 2
                    exit_dict["point"].append(exit_)
                    exit_dict["axis"].append(ax)
                    exit_dict["normal"].append(normal[i])
                    #     = {
                    #     "point": exit_,
                    #     "axis": ax
                    # }
                elif pair["rps_type"] == "伸缩销":
                    vertical = np.cross(ax, normal)
                    vertical = vertical / np.linalg.norm(vertical)
                    exit_ = target + vertical * 50 / 2 + normal * 25 / 2
                    exit_dict = {
                        "point": exit_,
                        "axis": vertical,
                        "vertical": ax,
                        "normal": normal,
                        "rps_type": "伸缩销"
                    }
        return exit_dict


class ConnectorGenerator():
    def __init__(self, clamp, work_mesh, entries_exits, resolution, gasket_thickness=5):
        self.maze = Maze(clamp + work_mesh, resolution=resolution)
        self.maze.set_boundaries(clamp.bounds.tolist())
        # self.maze.show()
        self.entries_exits = entries_exits
        self.res = resolution
        # self.adjust_times=adjust_times
        self.gasket_thickness = gasket_thickness
        self.path = self.get_path()

    def add_obstacles(self):
        center = (self.entries_exits[0][1]["point"] + np.array([0, 0, 10])).tolist()
        radius = [5]
        sphere = center + radius
        # self.maze.add_balls([[1598.66 ,  724.912, -643.627,5],[1614.91 ,  724.912, -736.127,5],[1559.66,724.912,-690.627,5],[1576.66,724.912,-708.627,5]])
        self.maze.add_balls([sphere])
        # self.maze.add_blocks()

    def extend_path(self, path, entry_axis, exit_axis, exits="axis", entry_length=65, exit_length=65):
        dir0 = path[0][1] - path[0][0]
        if all(abs(np.cross(dir0, entry_axis)) <= np.array([1e-10, 1e-10, 1e-10])):
            path[0][0] = path[0][0] - entry_axis * entry_length
        else:
            first = np.array([path[0][0] - entry_axis * entry_length, path[0][0]])
            path = np.insert(path, 0, values=first, axis=0)
        if "axis" not in exits or entry_axis is None:
            return path.tolist()
        elif isinstance(exit_axis, np.ndarray):
            dir_1 = path[-1][1] - path[-1][0]
            if all(abs(np.cross(dir_1, exit_axis)) <= np.array([1e-10, 1e-10, 1e-10])):
                path[-1][1] = path[-1][1] - exit_axis * exit_length
            else:
                last = np.array([[path[-1][1], path[-1][1] - exit_axis * exit_length]])
                path = np.concatenate((path, last), axis=0)
            return path.tolist()
        elif isinstance(exit_axis, list):
            pass

    def get_path(self):
        # self.add_obstacles()
        n = len(self.entries_exits[0])
        path_list = []

        if "vertical" in self.entries_exits[0][0]:
            entry = self.entries_exits[0][0]["point"]
            entry_axis = self.entries_exits[0][0]["axis"]
            entry_vertical = self.entries_exits[0][0]["vertical"]
            entry_normal = self.entries_exits[0][0]["normal"]
            exit_ = self.entries_exits[1][0]["point"]
            exit_axis = self.entries_exits[1][0]["axis"]
            exit_vertical = self.entries_exits[1][0]["vertical"]
            exit_normal = self.entries_exits[1][0]["normal"]

            diff_ = exit_ - entry
            s1 = np.inner(diff_, entry_normal)
            t1 = np.inner(diff_, entry_axis)
            s2 = np.inner(diff_, exit_vertical)
            path_1 = [[(entry - 50 * entry_axis).tolist(), (entry + (t1 - 25) * entry_axis).tolist()],
                      [(entry + (t1 - 25) * entry_axis).tolist(), (entry + (t1 - 25) * entry_axis + s1 * entry_normal).tolist()]]
            path_2 = [[(exit_ + 25 * exit_axis - (s2 - 25) * exit_vertical).tolist(), (exit_ + 25 * exit_axis).tolist()], [(exit_ + 25 * exit_axis).tolist(), (exit_ - 50 * exit_axis).tolist()]]
            path_list.append(path_1)
            path_list.append(path_2)
        # elif "adjust_times" in self.entries_exits[0][0]:

        else:
            for i in range(n):
                self.maze.set_start(self.entries_exits[0][i]["point"])
                if isinstance(self.entries_exits[1][i]["point"], np.ndarray):
                    # ent_point = trimesh.points.PointCloud([self.entries_exits[0][i]["point"],self.entries_exits[1][i]["point"]])
                    # scene = trimesh.Scene([self.maze.map, ent_point])
                    # scene.show()
                    if "normal" in self.entries_exits[1][i]:
                        self.maze.set_target(self.entries_exits[1][i]["point"] + self.gasket_thickness * self.entries_exits[1][i]["normal"])
                    else:
                        self.maze.set_target(self.entries_exits[1][i]["point"])
                    Astar = Weighted_A_star(self.maze, start_direction=self.entries_exits[0][i]["axis"], start_normal=self.entries_exits[0][i]["normal"])
                    Astar.run()
                    path = Astar.path3()
                    Cali = PathCalibrator(path, self.res)
                    path = Cali.to_new_path()

                    adjust_times = self.entries_exits[0][i]["adjust_times"]
                    if np.inner(self.entries_exits[1][i]["point"] - self.entries_exits[0][i]["point"], self.entries_exits[0][i]["axis"]) < 0:
                        if "axis" in self.entries_exits[1][i]:
                            # path=self.extend_path(path, -self.entries_exits[0][i]["axis"], -self.entries_exits[1][i]["axis"],exits="axis", entry_length=5, exit_length=5)
                            path = self.extend_path(
                                path, entry_axis=self.entries_exits[0][i]["axis"],
                                exit_axis=self.entries_exits[1][i]["axis"]
                                )
                        else:
                            path = path
                        PS = PathSplit(path, adjust_times)
                        for path in PS.get_split():
                            path_list.append(path)
                        # path_list.append(path)
                    else:
                        # ent_point = trimesh.points.PointCloud([self.entries_exits[0][i]["point"]])
                        # path = trimesh.load_path(path)
                        # scene = trimesh.Scene([self.maze.map, ent_point, path])
                        # scene.show()
                        # import pyrender
                        # scene = pyrender.Scene(bg_color=[0, 0, 0])
                        # scene.add(pyrender.Mesh.from_trimesh([self.maze.map], smooth=False))
                        # pyrender.Viewer(
                        #     scene,
                        #     use_raymond_lighting=True,
                        #     show_world_axis=True
                        # )
                        path = self.extend_path(path, entry_axis=self.entries_exits[0][i]["axis"], exit_axis=self.entries_exits[1][i]["axis"])

                        # ent_point = trimesh.points.PointCloud([self.entries_exits[0][i]["point"]])
                        # path = trimesh.load_path(path)
                        # scene = trimesh.Scene([self.maze.map, ent_point, path])
                        # scene.show()
                        # PS=PathSplit(path,adjust_times)
                        # path=PS.get_split()
                        PS = PathSplit(path, adjust_times)
                        for path in PS.get_split():
                            path_list.append(path)
                        # path_list.append(path)

                elif isinstance(self.entries_exits[1][i]["point"], list):
                    path_temp = [
                        [(self.entries_exits[1][i]["point"][0] - self.entries_exits[1][i]["axis"][0] * 65).tolist(),
                         (self.entries_exits[1][i]["point"][0]).tolist()],
                        [(self.entries_exits[1][i]["point"][0]).tolist(),
                         (self.entries_exits[1][i]["point"][1]).tolist()],
                        [(self.entries_exits[1][i]["point"][1]).tolist(),
                         (self.entries_exits[1][i]["point"][1] - self.entries_exits[1][i]["axis"][1] * 65).tolist()]]

                    # path_temp = np.array([[self.entries_exits[1][i]["point"][0],self.entries_exits[1][i]["point"][1]]])
                    # path_temp = np.array(self.extend_path(path_temp,entry_axis=self.entries_exits[1][i]["axis"][0],exit_axis=self.entries_exits[1][i]["axis"][1]))
                    plane_origin = self.entries_exits[0][i]["point"]
                    normal = np.cross(self.entries_exits[0][i]["axis"], self.entries_exits[0][i]["normal"])
                    plane_normal = normal
                    path_temp = np.array(path_temp)
                    # print(insec)
                    # path_3d = trimesh.load_path(path_temp)
                    # ent_point = trimesh.points.PointCloud(
                    #     [self.entries_exits[0][i]["point"], self.entries_exits[1][i]["point"][0],
                    #      self.entries_exits[1][i]["point"][1]])
                    #
                    # normal_3d = trimesh.load_path([plane_origin,plane_origin+100*normal])
                    # scene = trimesh.Scene([self.maze.map, path_3d, ent_point,normal_3d])
                    # scene.show()
                    # for path in path_temp:
                    insec0 = trimesh.intersections.plane_lines(
                        plane_origin,
                        plane_normal,
                        path_temp.transpose(1, 0, 2),
                        line_segments=True
                        )
                    # if len(insec0[0])!=0:
                    #     break

                    if len(insec0[0]) == 0:
                        target_list = [
                            (np.array(self.entries_exits[0][i]["point"]) - np.array(self.entries_exits[1][i]["point"][k])) for k in
                            range(2)]
                        # print(start_axis_list)
                        if np.linalg.norm(target_list[1]) > np.linalg.norm(target_list[0]):
                            target_point = self.entries_exits[1][i]["point"][1]
                            exit_axis = self.entries_exits[1][i]["axis"][1]
                        else:
                            target_point = self.entries_exits[1][i]["point"][0]
                            exit_axis = self.entries_exits[1][i]["axis"][0]
                        self.maze.set_target(target_point)
                        Astar = Weighted_A_star(self.maze, start_direction=self.entries_exits[0][i]["axis"], start_normal=self.entries_exits[0][i]["normal"])
                        Astar.run()
                        path = Astar.path3()
                        Cali = PathCalibrator(path, self.res)
                        path = Cali.to_new_path()
                        path_list.append(self.extend_path(path, self.entries_exits[0][i]["axis"], exit_axis))
                    else:
                        maze_temp = copy.deepcopy(self.maze)
                        maze_temp.set_start(self.entries_exits[1][i]["point"][0] + self.gasket_thickness * self.entries_exits[1][i]["normal"][0])
                        maze_temp.set_target(self.entries_exits[1][i]["point"][1] + self.gasket_thickness * self.entries_exits[1][i]["normal"][1])
                        Astar_temp = Weighted_A_star(maze_temp, start_direction=self.entries_exits[1][i]["axis"][0])
                        Astar_temp.run()
                        path_temp = Astar_temp.path3()
                        Cali = PathCalibrator(path_temp, self.res)
                        path = Cali.to_new_path()

                        if "rps_type" in self.entries_exits[1][i] and self.entries_exits[1][i]["rps_type"] == "connect":
                            path_list.append(path.tolist())
                        else:
                            # path_temp = self.extend_path(path, self.entries_exits[1][i]["axis"][0],
                            #                                   self.entries_exits[1][i]["axis"][1],self.entries_exits[1][i])
                            path_list.append(
                                self.extend_path(
                                    path, self.entries_exits[1][i]["axis"][0],
                                    self.entries_exits[1][i]["axis"][1], self.entries_exits[1][i]
                                    )
                                )
                        plane_origin = self.entries_exits[0][i]["point"]
                        normal = np.cross(self.entries_exits[0][i]["axis"], self.entries_exits[0][i]["normal"])
                        plane_normal = normal

                        path_temp = np.array(self.extend_path(path_temp, self.entries_exits[1][i]["axis"][0], self.entries_exits[1][i]["axis"][1]))
                        if len(path_temp) == 1:

                            insec = trimesh.intersections.plane_lines(
                                plane_origin,
                                plane_normal,
                                path_temp[0],
                                line_segments=True
                                )
                        else:
                            insec = trimesh.intersections.plane_lines(
                                plane_origin,
                                plane_normal,
                                path_temp.transpose(1, 0, 2),
                                line_segments=True
                                )

                        # path = trimesh.load_path(path_temp)

                        # print(insec)
                        # ent_point = trimesh.points.PointCloud([entries[0]["point"], exits[0]["point"]])
                        # scene = trimesh.Scene([maze_temp.map, path,ent_point])
                        # scene.show()
                        if len(insec[0]) == 0:
                            self.maze.set_target(self.entries_exits[1][i]["point"][0])
                            Astar = Weighted_A_star(self.maze, start_direction=self.entries_exits[0][i]["axis"])
                            Astar.run()
                            path = Astar.path3()
                            Cali = PathCalibrator(path, self.res)
                            path = Cali.to_new_path()
                            path_list.append(path.tolist())
                        else:
                            self.maze.set_target(insec[0][0])
                            Astar = Weighted_A_star(self.maze, start_direction=self.entries_exits[0][i]["axis"])
                            Astar.run()
                            path = Astar.path3()
                            Cali = PathCalibrator(path, self.res)
                            path = Cali.to_new_path()

                            dir0 = path[0][1] - path[0][0]
                            if all(np.cross(dir0, self.entries_exits[0][i]["axis"]) == np.array([0, 0, 0])):
                                path[0][0] = path[0][0] - self.entries_exits[0][i]["axis"] * 65
                            else:
                                first = np.array([path[0][0] - self.entries_exits[0][i]["axis"] * 65, path[0][0]])
                                path = np.insert(path, 0, values=first, axis=0)

                            dir_last = path[-1][1] - path[-1][0]
                            last_length = np.linalg.norm(dir_last)
                            dir_last = dir_last / last_length

                            if last_length <= 25:
                                path_new = path.tolist()
                                path_new.pop()
                                path_list.append(path_new)
                                dir_last = np.array(path_new[-1][1]) - np.array(path_new[-1][0])
                                dir_last = dir_last / np.linalg.norm(dir_last)
                                path_list.append(
                                    [[(insec[0][0] - dir_last * 10).tolist(),
                                      (insec[0][0] - dir_last * 65).tolist()]]
                                    )

                            else:
                                move_dir = np.cross(normal, dir_last)
                                for j in range(len(path)):
                                    if j != 0:
                                        path[j][0] = path[j][0] + move_dir * 20
                                    path[j][1] = path[j][1] + move_dir * 20
                                path_list.append(path.tolist())
                                path_list.append([[(insec[0][0] - dir_last * 10).tolist(), (insec[0][0] - dir_last * 65).tolist()]])
        # self.path = path_list
        return path_list

    def get_maze(self):
        return self.maze

    def get_compound(self):
        # path = trimesh.load_path(self.path[0])
        # scene = trimesh.Scene([self.maze.map,path])
        # scene.show()
        prisms_list = []
        if 'rps_type' in self.entries_exits[0][0] and self.entries_exits[0][0]['rps_type'] == "伸缩销":
            path_list = self.path
            w = [25, 50]
            d = [50, 25]
            for i in range(len(path_list)):
                Ps = PathPrism(path_list[i], dx=w[i])
                Ps.init()
                cycles = Ps.cycle()
                for j in range(len(cycles)):
                    cycles[j] = cycles[j] - Ps.unit_normal * d[i] / 2
                PB = PrismBuilder(cycles, Ps.unit_normal, d[i])
                prism = PB.build_prism()
                prisms_list.append(prism)
        else:
            for path in self.path:
                # path_3d = trimesh.load_path(path)
                # scene = trimesh.Scene([self.maze.map,path_3d])
                # scene.show()
                x = 20
                y = 20
                Ps = PathPrism(path, dx=x)
                Ps.init()
                cycles = Ps.cycle()
                for i in range(len(cycles)):
                    cycles[i] = cycles[i] - Ps.unit_normal * y / 2
                PB = PrismBuilder(cycles, Ps.unit_normal, y)
                prism = PB.build_prism()
                prisms_list.append(prism)
        return prisms_list

    def show_paths(self):
        for path in self.path:
            path_3d = trimesh.load_path(path)
            scene = trimesh.Scene([self.maze.map, path_3d])
            scene.show()

    def to_json(self):
        path_list = []
        # if self.get_path()[-1] == "伸缩销":
        if 'rps_type' in self.entries_exits[0][0] and self.entries_exits[0][0]['rps_type'] == "伸缩销":
            paths = self.path
            w = [25, 50]
            d = [50, 25]
            for i in range(len(paths)):
                path_dict = {}
                path = paths[i]
                Ps = PathPrism(path, dx=w[i])
                Ps.init()
                cycles = Ps.cycle()

                origin = path[0][0]
                dir_h = Ps.get_dirs()[0] / np.linalg.norm(Ps.get_dirs()[0])
                dir_v = Ps.unit_verticals[0]

                for j in range(len(cycles)):
                    cycles[i] = cycles[i] - Ps.unit_normal * d[i] / 2

                local = G2L(origin, dir_h, dir_v, cycles)
                path_dict["sketchOrigin"] = (path[0][0] - Ps.unit_normal * d[i] / 2).tolist()
                path_dict["sketchDirection_H"] = dir_h.tolist()
                path_dict["sketchDirection_V"] = dir_v.tolist()
                path_dict["sketchPoint_Position"] = local
                path_dict["padDirection"] = Ps.unit_normal.tolist()
                path_dict["padLimit"] = d[i]
                path_list.append(path_dict)
        else:
            for path in self.path:
                path_dict = {}
                x = 20
                y = 20
                Ps = PathPrism(path, dx=x)
                Ps.init()
                cycles = Ps.cycle()

                origin = path[0][0]
                dir_h = Ps.get_dirs()[0] / np.linalg.norm(Ps.get_dirs()[0])
                dir_v = Ps.unit_verticals[0]

                for i in range(len(cycles)):
                    cycles[i] = cycles[i] - Ps.unit_normal * y / 2

                local = G2L(origin, dir_h, dir_v, cycles)
                # control_point = G2L(origin,dir_h,dir_v,path)
                path_dict["sketchOrigin"] = (path[0][0] - Ps.unit_normal * y / 2).tolist()
                path_dict["sketchDirection_H"] = dir_h.tolist()
                path_dict["sketchDirection_V"] = dir_v.tolist()
                path_dict["sketchPoint_Position"] = local
                path_dict["padDirection"] = Ps.unit_normal.tolist()
                path_dict["padLimit"] = 20.0
                path_dict["control_point"] = None
                path_list.append(path_dict)
        return path_list
